#include "MPC5604B_M07N.h"
#define TRUE 1
#define FALSE 0

void init_motor(void)
{
    SIU.PCR[74].R = 0x0200;
   SIU.PCR[75].R = 0x0200;
   SIU.PCR[76].R = 0x0200;
   SIU.PCR[77].R = 0x0200;
   
}

void MOTOR_LEFT(void)
{
   
   SIU.GPDO[74].B.PDO = 0;
   SIU.GPDO[75].B.PDO = 1;
   
   
   //SIU.PCR[16].R = 0x0200;            /* Program the drive enable pin of Left Motor as output*/
   //SIU.PGPDO[0].R = 0x00008000;      /* Enable Left the motors */
   //Delaywait();
   //SIU.PGPDO[0].R = 0x00000000;      /* Disable Left the motors */
}

void MOTOR_RIGHT(void)
{
   SIU.GPDO[76].B.PDO = 0;
   SIU.GPDO[77].B.PDO = 1;

   //SIU.PCR[17].R = 0x0200;            /* Program the drive enable pin of Right Motor as output*/
//   SIU.PGPDO[0].R = 0x00004000;      /* Enable Right the motors */
//   Delaywait();
//   SIU.PGPDO[0].R = 0x00000000;      /* Disable Right the motors */
}
void set_motores(int veloc1, int veloc2)
{
   EMIOS_0.CH[6].CBDR.R = veloc1;
   EMIOS_0.CH[5].CBDR.R = veloc2;
}

void reverse(int motorLeft,int motorRight)
{
   if(motorLeft==TRUE)
   {
      SIU.GPDO[74].B.PDO = 1;
      SIU.GPDO[75].B.PDO = 0;
   }
   if(motorRight==TRUE)
   {
      SIU.GPDO[76].B.PDO = 1;
      SIU.GPDO[77].B.PDO = 0;
   }
}

void direto(int motorLeft, int motorRight)
{
   if(motorLeft==TRUE)
   {
      SIU.GPDO[74].B.PDO = 0;
      SIU.GPDO[75].B.PDO = 1;
   }
   if(motorRight==TRUE)
   {
      SIU.GPDO[76].B.PDO = 0;
      SIU.GPDO[77].B.PDO = 1;
   }
}

void freio(int motorLeft, int motorRight)
{
   if(motorLeft==TRUE)
   {
      SIU.GPDO[74].B.PDO = 0;
      SIU.GPDO[75].B.PDO = 0;
   }
   if(motorRight==TRUE)
   {
      SIU.GPDO[76].B.PDO = 0;
      SIU.GPDO[77].B.PDO = 0;
   }
}

/*
void RIGHT_MOTOR_CURRENT(void)
{
	
	SIU.PGPDO[0].R = 0x00004000;		
	Delaywait();
	for (i=0;i <10;i++)
	{
		ADC.MCR.B.NSTART=1;     			
		while (ADC.MSR.B.NSTART == 1) {};
		curdata = ADC.CDR[2].B.CDATA;
		printserialsingned(curdata);		
	}
	SIU.PGPDO[0].R = 0x00000000;		
}

void LEFT_MOTOR_CURRENT(void)
{
	
	SIU.PGPDO[0].R = 0x00008000;			
	Delaywait();
	for (i=0;i <10;i++)
	{
		ADC.MCR.B.NSTART=1;     			
		while (ADC.MSR.B.NSTART == 1) {};
		curdata = ADC.CDR[1].B.CDATA;
		printserialsingned(curdata);		
	}
	SIU.PGPDO[0].R = 0x00000000;		
}
*/